'''
This demo show the communication interface of MR813 motion control board based on Lcm.
Dependency: 
- robot_control_cmd_lcmt.py
- robot_control_response_lcmt.py
'''
import lcm
import sys
import os
import time
from threading import Thread, Lock

from robot_control_cmd_lcmt import robot_control_cmd_lcmt
from robot_control_response_lcmt import robot_control_response_lcmt

def main():
    Ctrl = Robot_Ctrl()
    Ctrl.run()

class Robot_Ctrl(object):
    def __init__(self):
        self.rec_thread = Thread(target=self.rec_responce)
        self.send_thread = Thread(target=self.send_publish)
        self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
        self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
        self.cmd_msg = robot_control_cmd_lcmt()
        self.rec_msg = robot_control_response_lcmt()
        self.msg = robot_control_cmd_lcmt()
        self.send_lock = Lock()
        self.delay_cnt = 0
        self.mode_ok = 0
        self.gait_ok = 0
        self.runing = 1

        self.buffer = 0
        self.status = 0
        self.is_lifted = 0

        self.stage = 0

        self.motion_thread = Thread(target=self.motion,args=(self.stage,))
        self.update_status_thread = Thread(target=self.update_status,args = ())
        self.guard_thread = Thread(target=self.guard,args = ())
        
    def motion(self,stage):
        try:
            test_speed = 0.1
            test_duration = 5000
            while stage == 0:  
                self.msg.mode = 12 # Recovery stand
                self.msg.gait_id = 0
                self.msg.life_count += 1 # Command will take effect when life_count update
                self.Send_cmd(self.msg)
                self.Wait_finish(12, 0)
                stage += 1
                self.stage += 1
                print("up to", stage)
            
            while stage == 1:###前
                self.msg.mode = 11 # Recovery stand
                self.msg.gait_id = 27
                self.msg.vel_des = [test_speed, 0, 0]  # forward  left/rightmove  rotate
                self.msg.duration = test_duration
                self.msg.life_count += 1 # Command will take effect when life_count update
                self.Send_cmd(self.msg)
                time.sleep(12)
                stage += 1
                self.stage += 1
                print("up to", stage)


            while stage == 2:###左
                self.msg.mode = 11 # Recovery stand
                self.msg.gait_id = 27
                self.msg.vel_des = [0, test_speed, 0]  # forward  left/rightmove  rotate
                self.msg.duration = test_duration
                self.msg.life_count += 1 # Command will take effect when life_count update
                self.Send_cmd(self.msg)
                time.sleep(12)
                stage += 1
                self.stage += 1
                print("up to", stage)

            while stage == 3:  ####后
                self.msg.mode = 11 # Recovery stand
                self.msg.gait_id = 27
                self.msg.vel_des = [-test_speed, 0, 0]  # forward  left/rightmove  rotate
                self.msg.duration = test_duration
                self.msg.life_count += 1 # Command will take effect when life_count update
                self.Send_cmd(self.msg)
                time.sleep(12)
                stage += 1
                self.stage += 1
                print("up to", stage)

            while stage == 4: ###右
                self.msg.mode = 11 # Recovery stand
                self.msg.gait_id = 27
                self.msg.vel_des = [0, -test_speed, 0]  # forward  left/rightmove  rotate
                self.msg.duration = test_duration
                self.msg.life_count += 1 # Command will take effect when life_count update
                self.Send_cmd(self.msg)
                time.sleep(12)
                stage += 1
                self.stage += 1
                print("up to", stage)
            
        except KeyboardInterrupt:
            pass
            self.quit()
            sys.exit()

    def motion_quit(self):
        # self.runing = 0 ###要不要锁死？
        self.motion_thread.join()

    def motion_restart(self,stage):
        self.motion_thread = Thread(target=self.motion,args=(stage,))
        # self.runing = 1 ###要不要锁死？
        self.motion_thread.start()
        print("restart", stage)

    def update_status(self):
        while True:
            print(self.is_lifted)
            self.buffer = self.is_lifted  ###保存之前的值
            if self.rec_msg.switch_status == 3:  ##摔倒
                self.is_lifted = 3
            elif self.rec_msg.switch_status == 4:   ###提起
                self.is_lifted = 1  ###更新新值
            else:     #####正常运动0  或者   运动过程1
                self.is_lifted = 0  ###更新新值
            
            # print('status?   ',self.rec_msg.switch_status)
            time.sleep(0.08)

    def guard(self):
        while True:
        # 含义 0 是正常，  1 被抬起     3 是高阻尼态
            if self.buffer == 0 and (self.is_lifted == 3 or self.is_lifted == 1):  ###正常运动到挂掉
                self.motion_quit()
                print("killed", stage)
                time.sleep(0.1)

                while self.is_lifted == 3:
                    self.msg.mode = 11 # 随便发个指令更新状态
                    self.msg.gait_id = 27
                    self.msg.vel_des = [0, 0, 0]  # forward  left/rightmove  rotate
                    self.msg.duration = 100
                    self.msg.life_count += 1
                    self.Send_cmd(self.msg)
                    time.sleep(1)
            
                while self.is_lifted == 1:  ###提起之后随时监测是否落地
                    self.msg.mode = 12 # Recovery stand
                    self.msg.gait_id = 0
                    self.msg.life_count += 1 # Command will take effect when life_count update
                    self.Send_cmd(self.msg)
                    time.sleep(1)
                    print("monitor", stage)
            
                self.Wait_finish(12,0)
                self.motion_restart(self.stage)
                time.sleep(0.1)
            time.sleep(0.03)

    def run(self):
        self.lc_r.subscribe("robot_control_response", self.msg_handler)
        self.send_thread.start()
        self.rec_thread.start()
        self.update_status_thread.start()
        self.guard_thread.start()
        self.motion_thread.start()
        
    def msg_handler(self, channel, data):
        self.rec_msg = robot_control_response_lcmt().decode(data)
        if(self.rec_msg.order_process_bar >= 95):
            self.mode_ok = self.rec_msg.mode
        else:
            self.mode_ok = 0

    def rec_responce(self):
        while self.runing:
            self.lc_r.handle()
            time.sleep( 0.002 )

    def Wait_finish(self, mode, gait_id):
        count = 0
        while self.runing and count < 2000: #10s
            if self.mode_ok == mode and self.gait_ok == gait_id:
                return True
            else:
                time.sleep(0.005)
                count += 1

    def send_publish(self):
        while self.runing:
            self.send_lock.acquire()
            if self.delay_cnt > 20: # Heartbeat signal 10HZ, It is used to maintain the heartbeat when life count is not updated
                self.lc_s.publish("robot_control_cmd",self.cmd_msg.encode())
                self.delay_cnt = 0
            self.delay_cnt += 1
            self.send_lock.release()
            time.sleep( 0.005 )

    def Send_cmd(self, msg):
        self.send_lock.acquire()
        self.delay_cnt = 50
        self.cmd_msg = msg
        self.send_lock.release()

    def quit(self):
        self.runing = 0
        self.rec_thread.join()
        self.send_thread.join()
        self.update_status_thread.join()
        self.guard_thread.join()
        self.motion_thread.join()

# Main function
if __name__ == '__main__':
    main()